#include "SerialModbus.h"


SerialModbus::SerialModbus(std::string path,std::string baud_rate):SerialPort(path,baud_rate)
{
    dev_addr=0x01;//默认从站地址
}

SerialModbus::~SerialModbus()
{
}

void SerialModbus::printf_command(std::vector<unsigned char>& com,std::string tap){
    std::cout << tap;
    for (const auto& element:com) {
        std::cout.width(2);
        std::cout.fill('0');
        std::cout << std::hex << static_cast<int>(element) << " ";
    }
    std::cout << std::endl;
}

//发送指令
int SerialModbus::send(std::vector<unsigned char>& command){
    int ret=0;

    //校验和计算结果添加
    compute_CRC16(command);

    //显示最终的命令
    printf_command(command,"发送数据:");

    //发送
    ret = writeData((const char *)command.data(),command.size());

    return ret;
}
//发送指令
int SerialModbus::send_command(unsigned char devaddr,
                unsigned char code,
                std::vector<unsigned char> addr,
                std::vector<unsigned char> len){

    std::vector<unsigned char> com_a;
    com_a.clear();

    //组装命令
    com_a.push_back(devaddr);//设备地址
    com_a.push_back(code);//功能码

    //插入起始地址
    com_a.insert(com_a.end(),addr.begin(),addr.end());
    //插入偏移量
    com_a.insert(com_a.end(),len.begin(),len.end());
    //计算并插入CRC校验结果
    compute_CRC16(com_a);

    //显示最终的命令
    printf_command(com_a,"send_command data:");

    //发送
    writeData((const char *)com_a.data(),com_a.size());

    return 0;
}

//接收指令
std::vector<unsigned char> SerialModbus::receive(int len){
    std::vector<unsigned char> data;
    data.clear();
    int ret = -1;
    
    while (1)
    {
        unsigned char t;
        ret=readData_one(&t);
        if(ret >= 1){
            // printf(">>> readData_one is sum :%d %02x \n\r",sum,t);
            data.push_back(t);
            if (data.size() >= len)
            {
                printf_command(data,"接收的数据：");
                break;
            }
        }  
    }

    //校验数据
    ret = check_CRC16(data);
    if (ret !=1)
    {
        std::cout << "CRC校验错误!" << std::endl;
        data.clear();
    }
    
    return data;
}

int CRC16_modbus(std::vector<unsigned char>& com){
    uint16_t crc = 0xFFFF; // initial value
    for (uint8_t byte : com) {
        crc ^= byte;
        for (int i = 0; i < 8; ++i) {
            if (crc & 0x0001) {
                crc >>= 1;
                crc ^= 0xA001;
            } else {
                crc >>= 1;
            }
        }
    }
    crc &= 0xFFFF;

    return crc;
}

//计算CRC16校验和
void SerialModbus::compute_CRC16(std::vector<unsigned char>& com){
    uint16_t crc = 0xFFFF; // initial value
    for (uint8_t byte : com) {
        crc ^= byte;
        for (int i = 0; i < 8; ++i) {
            if (crc & 0x0001) {
                crc >>= 1;
                crc ^= 0xA001;
            } else {
                crc >>= 1;
            }
        }
    }
    crc &= 0xFFFF;

    // printf(">>> crc result:%02x,%02x\n\r",(crc >> 8) & 0xFF,crc & 0xFF);
    com.push_back(crc & 0xFF);
    com.push_back((crc >> 8) & 0xFF);
    
}

bool SerialModbus::check_CRC16(std::vector<unsigned char>& com){
    int len = com.size();

    //  获取CRC结果
    uint16_t s_crc=(com[len-1]<<8)+com[len-2];

    // 重新计算CRC
    auto it = std::next(com.begin(), std::distance(com.begin(), com.end()) - 2);
    std::vector<unsigned char> sub_vec(com.begin(),it);
    uint16_t crc = 0xFFFF; // initial value
    for (uint8_t byte : sub_vec) {
        crc ^= byte;
        for (int i = 0; i < 8; ++i) {
            if (crc & 0x0001) {
                crc >>= 1;
                crc ^= 0xA001;
            } else {
                crc >>= 1;
            }
        }
    }
    crc &= 0xFFFF;

    //判断CRC是否一致
    if(crc == s_crc){
        return true;
    }else{
        return false;
    }
}


